/*
 * SsrrController.h
 *
 *  Created on: Oct 23, 2013
 *      Author: incubed
 */

#ifndef SSRRCONTROLLER_H_
#define SSRRCONTROLLER_H_

#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>

namespace ssrr {

class SsrrController {
public:
	SsrrController(double trans_forward, double max_trans, double escape_trans, double min_rot, double max_rot);
	~SsrrController();

	bool control(double pos_x, double pos_y, double pos_th, double cur_trans, double cur_rot,  double &result_trans, double &result_rot, geometry_msgs::PoseStamped& goal, nav_msgs::Path &path);

private:
	SsrrController(const SsrrController& other) { }

	double getGoalCost(double x, double y, double th, geometry_msgs::PoseStamped &pose);
	double getPathCost(double x, double y, double th, nav_msgs::Path &path);
	double getObstacleCost(double last_x, double last_y, double last_th, double x, double y, double th);

	bool escape(double x, double y, double th);
	bool rotate(double x, double y, double th, bool right);
	bool moveBack(double x, double y, double th);
	Trajectory createTrajectory(double x, double y, double th, double trans, double rot, double sim_time, double sim_step);

	inline double dist(double start_x, double start_y, double end_x, double end_y) {
		return std::sqrt((start_x - end_x) * (start_x - end_x) + (start_y - end_y) * (start_y - end_y));
	}

	double escape_trans_;
	double min_trans_;
	double max_trans_;
	double min_rot_;
	double max_rot_;
};

}



#endif /* SSRRCONTROLLER_H_ */
